#include "StdAfx.h"
#include "StrokeBuilder.h"
#include "WritingTrajectoire.h"

StrokeBuilder::StrokeBuilder(void)
{

}

// Build SPWT (Sample Points of Writing Trajectoires) for each substroke
void StrokeBuilder::build_spwt(char* source_img_name, vector<StrokeNode*> stroke_graph)
{
	// Load source image
	IplImage* source_image = cvLoadImage(source_img_name,CV_LOAD_IMAGE_GRAYSCALE);
	
	for( int node_index = 0; node_index < (int) stroke_graph.size(); node_index++)
	{
		if((!stroke_graph[node_index]->isAmbigousZone) && (stroke_graph[node_index]->starting_pointers.size() > 0))
		{
			this->build_stroke_spwt(source_image,stroke_graph[node_index]);
			map<StrokeNode*, vector<CvPoint>>::iterator it;
			for (it = stroke_graph[node_index]->spwt.begin(); it != stroke_graph[node_index]->spwt.end(); it++)
			{
				StrokeNode* ambigous_zone = (*it).first;
				for (int j = 0; j < (int) (*it).second.size() - 1; j++)
				{
					CvPoint p0 =  (*it).second[j];
					CvPoint p1 =  (*it).second[j + 1];
					cvLine(source_image,p0,p1,cvScalar(150,150,150));
					//cvSet2D(source_image,p0.y, p0.x,cvScalar(150,150,150));
					//cvSet2D(source_image,p1.y, p1.x,cvScalar(150,150,150));
				}
			}
		}
	}
	cvSaveImage("spwt.png",source_image);
	// Release source image
	cvReleaseImage(&source_image);
	return;
}

// Build SPWT for a substroke
void StrokeBuilder::build_stroke_spwt(IplImage* source_image, StrokeNode* stroke)
{
/* ---------- Find contours of current substroke ---------- */
	
	// Image contains substrokes (except ambigous zones)
	IplImage *img_edge = cvCreateImage(cvSize(source_image->width,source_image->height),source_image->depth,source_image->nChannels);
	cvZero(img_edge);
	// Paint color
	CvScalar white = cvScalar(255,255,255);

	set<CvPoint,ImagePointComp>::iterator it;
	for(it = stroke->elements.begin(); it != stroke->elements.end(); it++)
	{
		cvSet2D(img_edge,(*it).y,(*it).x,white);
	}

	CvMemStorage* storage = cvCreateMemStorage(); 
	
	CvChain* first_contour = NULL;

	// Find contours
	int Nc = cvFindContours(
		img_edge,
		storage,
		(CvSeq**)&first_contour,
		sizeof(CvChain),
		CV_RETR_LIST,
		CV_CHAIN_CODE);
	
	// List of contour
	vector<LinkedList*> contour_list;

	contour_list.clear();

	for( CvChain* c=first_contour; c!=NULL; c=(CvChain*)c->h_next )
	{
		LinkedList *contour = new LinkedList();

		CvChainPtReader reader;
		int i, total = c->total;
		if (c->total > 0)
		{
			cvStartReadChainPoints( c, &reader);
			for( i = 0; i < total; i++ )
			{
				CvPoint point;
				CV_READ_CHAIN_POINT(point, reader);
				int code = reader.code;
				contour->insert(point, code);
			}
			contour_list.push_back(contour);
		}
		
	}
	cvReleaseImage(&img_edge);
	for( CvChain* c=first_contour; c!=NULL; c=(CvChain*)c->h_next )
	{
		cvClearSeq((CvSeq*)c);
	}
	cvClearMemStorage(storage);
	cvReleaseMemStorage(&storage);
/* ---------- End find contours of curent stroke ---------- */


	// For each pair of starting pointers
	// Create its SPWT
	map<StrokeNode*,pair<CvPoint,CvPoint>>::iterator ite;

	for (ite = stroke->starting_pointers.begin(); ite != stroke->starting_pointers.end(); ite++)
	{
		StrokeNode* ambigous_zone = (*ite).first;
		vector<CvPoint> path;
		path.clear();
		CvPoint p0 = stroke->starting_pointers[ambigous_zone].first;
		CvPoint p1 = stroke->starting_pointers[ambigous_zone].second;
		int step = 0;
		double ds = 0.0;
		
		set<CvPoint,ImagePointComp> traced;
		NodeList* np0 = NULL;
		NodeList* np1 = NULL;
		LinkedList* p0_contour;
		LinkedList* p1_contour;
		for (int lidx = 0; lidx < (int) contour_list.size(); lidx++)
		{
			NodeList *temp = contour_list[lidx]->get(p0);
			if (temp != NULL)
			{
				np0 = temp;
				p0_contour = contour_list[lidx];
			}
			temp = contour_list[lidx]->get(p1);
			if (temp != NULL)
			{
				np1 = temp;
				p1_contour = contour_list[lidx];
			}

		}
		NodeList* p0x;
		NodeList* p1x;
		bool stop = false;
		NodeList* original_p0 = np0;
		NodeList* original_p1 = np1;
		for (int direction = 0; direction < 2; direction++)
		{
			vector<CvPoint> temp_path;
			temp_path.clear();
			traced.clear();
			stop = false;
			do
			{
				
				CvPoint centerPoint;
				centerPoint.x = (np0->data.x + np1->data.x) / 2;
				centerPoint.y = (np0->data.y + np1->data.y) / 2;
				temp_path.push_back(centerPoint);
				
				ds = Tools::distance_Euclidean(np0->data,np1->data);
				step = (int) floor(ds / 2 + 0.5);
				if (step == 0)
				{
					stop = true;
					break;
				}
				traced.insert(np0->data);
				traced.insert(np1->data);
				vector<NodeList*> forward_list = p0_contour->forward(np0,step);
				vector<NodeList*> backward_list = p1_contour->backward(np1,step);
				for (int i = 1; i< (int) forward_list.size(); i++)
				{
					if(traced.find(forward_list[i]->data) != traced.end())
					{
						stop = true;
						break;
					}
				}
				if (!stop)
				{
					for (int i = 1; i< (int) backward_list.size(); i++)
					{
						if(traced.find(backward_list[i]->data) != traced.end())
						{
							stop = true;
							break;
						}
					}
					if (!stop)
					{
						p0x = forward_list.back();
						p1x = backward_list.back();
						double d0 = Tools::distance_Euclidean(p0x->data,p1x->data);
						double d1 = Tools::distance_Euclidean(p0x->data,np1->data);
						double d2 = Tools::distance_Euclidean(np0->data, p1x->data);
						if ((d0 <= d1) && (d0 <= d2))
						{
							np0 = p0x;
							np1 = p1x;
							for (int i = 0; i< (int) forward_list.size() - 1; i++)
							{
								traced.insert(forward_list[i]->data);
							}
							for (int i = 0; i< (int) backward_list.size() - 1; i++)
							{
								traced.insert(backward_list[i]->data);
							}
						}
						else if ((d1 <= d0) && (d1 <= d2))
						{
							np0 = p0x;
							for (int i = 0; i< (int) forward_list.size() - 1; i++)
							{
								traced.insert(forward_list[i]->data);
							}
						}
						else if ((d2 <= d0) && (d2 <= d1))
						{
							np1 = p1x;
							for (int i = 0; i< (int) backward_list.size() - 1; i++)
							{
								traced.insert(backward_list[i]->data);
							}
						}

					}
				}

			}
			while (!stop);
			if (temp_path.size() > 2)
			{
				//path.insert(path.begin(), temp_path.begin(), temp_path.end());
				path = temp_path;
			}
			np0 = original_p1;
			np1 = original_p0;
			LinkedList* temp = p0_contour;
			p0_contour = p1_contour;
			p1_contour = temp;
		}
		stroke->spwt[ambigous_zone] = path;
	}
	
	// Delete contour list
	for (int i = 0; i< (int) contour_list.size(); i++)
	{
		delete contour_list[i];
	} 
	contour_list.clear();
}

// Refine stroke graph
void StrokeBuilder::refine_stroke_graph(vector<StrokeNode*> stroke_graph)
{
	// Merge 2 joined ambigous zones
	for( int node_index = 0; node_index < (int) stroke_graph.size(); node_index++)
	{
		StrokeNode* currentNode = stroke_graph[node_index];

		if (currentNode->isAmbigousZone)
		{
			vector<StrokeNode*> new_neighbours;
			new_neighbours.clear();

			set<int> delete_index;
			delete_index.clear();
			for (int neighbour_idx = 0; neighbour_idx < (int) currentNode->neighbours.size(); neighbour_idx++)
			{
				StrokeNode* neighbourNode = currentNode->neighbours[neighbour_idx];
				if (neighbourNode->isAmbigousZone)
				{
					currentNode->elements.insert(neighbourNode->elements.begin(), neighbourNode->elements.end());
					for (int i = 0; i< (int) neighbourNode->neighbours.size(); i++)
					{
						StrokeNode* nextNeighbour = neighbourNode->neighbours[i];
						if (nextNeighbour != currentNode)
						{
							new_neighbours.push_back(nextNeighbour);
							for (int j = 0; j < (int) nextNeighbour->neighbours.size(); j++)
							{
								if (nextNeighbour->neighbours[j] == neighbourNode)
								{
									nextNeighbour->neighbours[j] = currentNode;
								}
							}
							if (!nextNeighbour->isAmbigousZone)
							{
								vector<CvPoint> path = nextNeighbour->spwt[neighbourNode];
								pair<CvPoint,CvPoint> starting_pointers = nextNeighbour->starting_pointers[neighbourNode];
								nextNeighbour->spwt[currentNode] = path;
								nextNeighbour->starting_pointers[currentNode] = starting_pointers;
								nextNeighbour->spwt.erase(neighbourNode);
								nextNeighbour->starting_pointers.erase(neighbourNode);

							}
						}
					}
					delete_index.insert(neighbour_idx);
					delete neighbourNode;
					neighbourNode = NULL;
				}

			}

			for (int neighbour_idx = (int) currentNode->neighbours.size() - 1; neighbour_idx >= 0; neighbour_idx--)
			{
				if (delete_index.find(neighbour_idx) == delete_index.end())
				{
					new_neighbours.insert(new_neighbours.begin(),currentNode->neighbours[neighbour_idx]);
				}
			}
			
		}
	}

	// Clean NULL nodes in stroke graph
	for( int node_index = 0; node_index < (int) stroke_graph.size(); node_index++)
	{
		StrokeNode* currentNode = stroke_graph[node_index];
		if (currentNode == NULL)
		{
			stroke_graph.erase(stroke_graph.begin() + node_index);
			node_index = -1;
		}
	}
	return;
}

vector<vector<StrokeNode*>> StrokeBuilder::search_path(vector<StrokeNode*> stroke_graph)
{
	vector<vector<StrokeNode*>> retval;
	retval.clear();

	int count = 0;
	for( int node_index = 0; node_index < (int) stroke_graph.size(); node_index++)
	{
		if (stroke_graph[node_index]->isAmbigousZone)
		{
			StrokeNode* ambigous_zone = stroke_graph[node_index];
			int x0 = (int) ambigous_zone->neighbours.size();
			for (int i = 0; i < (int) ambigous_zone->neighbours.size() - 1; i++)
			{
				for (int j = i + 1; j < (int) ambigous_zone->neighbours.size(); j++)
				{
					StrokeNode* vi = ambigous_zone->neighbours[i];
					StrokeNode* vj = ambigous_zone->neighbours[j];
					if ((vi->spwt[ambigous_zone].size() > 0) &&
						(vj->spwt[ambigous_zone].size() > 0))
					{
						WritingTrajectoire spwt_i(vi->spwt[ambigous_zone],vi->starting_pointers[ambigous_zone].first, vi->starting_pointers[ambigous_zone].second);
						WritingTrajectoire spwt_j(vj->spwt[ambigous_zone],vj->starting_pointers[ambigous_zone].first, vj->starting_pointers[ambigous_zone].second);
						float x1 = spwt_i.get_angle_deviation(spwt_j);
						
						float min_deviation1 = 1000;
						float min_deviation2 = 1000;
						for (int k = 0; k < (int) ambigous_zone->neighbours.size(); k++)
						{
							
							StrokeNode* vk = ambigous_zone->neighbours[k];
							if ((vk != vi) && (vk != vj))
							{
								if (vk->spwt[ambigous_zone].size() > 0)
								{
									WritingTrajectoire spwt_k(vk->spwt[ambigous_zone],vk->starting_pointers[ambigous_zone].first, vk->starting_pointers[ambigous_zone].second);
									float angle_deviation1 = spwt_i.get_angle_deviation(spwt_k);
									float angle_deviation2 = spwt_j.get_angle_deviation(spwt_k);
									if (min_deviation1 > angle_deviation1)
									{
										min_deviation1 = angle_deviation1;
									}
									if (min_deviation2 > angle_deviation2)
									{
										min_deviation2 = angle_deviation2;
									}
								}
							}
						}
						float x2 = min(min_deviation1, min_deviation2);
						float x3 = abs(spwt_i.w - spwt_j.w);
						float x4 = WritingTrajectoire::get_curvature_variation(spwt_i,spwt_j);
						cout << x0 << " ; " << x1 << " ; " << x2 << " ; " << x3 << " ; " << x4 << endl;
						if (this->decide(x0,x1,x2,x3,x4))
						{
							vector<StrokeNode*> path;
							path.clear();
							vi->isolated = false;
							vj->isolated = false;
							ambigous_zone->isolated = false;
							path.push_back(vi);
							path.push_back(ambigous_zone);
							path.push_back(vj);
							retval.push_back(path);
						}
						count++;
					}
				}
			}
		}
	}

	vector<vector<StrokeNode*>>::iterator it;
	cout << retval.size() << endl;
	for (it = retval.begin(); it!= retval.end(); it++)
	{
		bool canMerge = true;
		bool changed = false;
		vector<vector<StrokeNode*>>::iterator current_pointer = it;
		current_pointer++;
		while(canMerge)
		{
			if (current_pointer == retval.end())
			{
				if (changed)
				{
					current_pointer = it;
					current_pointer++;
					changed = false;
				}
				else
				{
					canMerge = false;
				}
			}
			else
			{
				vector<StrokeNode*> temp1 = (*it);
				vector<StrokeNode*> temp2 = (*current_pointer);
				if (temp1[temp1.size() - 1] == temp2[0])
				{
					for (int i = 1; i < (int) temp2.size(); i++)
					{
						(*it).push_back(temp2[i]);
					}
					retval.erase(current_pointer);
					current_pointer = it;
					current_pointer++;
					changed = true;
				}
				else if (temp1[temp1.size() - 1] == temp2[temp2.size() - 1])
				{
					for (int i = temp2.size() - 2; i >= 0; i--)
					{
						(*it).push_back(temp2[i]);
					}
					retval.erase(current_pointer);
					current_pointer = it;
					current_pointer++;
					changed = true;
				}
				else if (temp1[0] == temp2[temp2.size() - 1])
				{
					for (int i = temp2.size() - 2; i >= 0; i--)
					{
						(*it).insert((*it).begin(),temp2[i]);
					}
					retval.erase(current_pointer);
					current_pointer = it;
					current_pointer++;
					changed = true;
				}
				else if (temp1[0] == temp2[0])
				{
					for (int i = 1; i < (int) temp2.size(); i++)
					{
						(*it).insert((*it).begin(),temp2[i]);
					}
					retval.erase(current_pointer);
					current_pointer = it;
					current_pointer++;
					changed = true;
				}
				else
				{
					current_pointer++;
				}
			}
		}


	}
	cout << retval.size() << endl;
	count = 0;
	for (int i = 0; i< (int) retval.size(); i++)
	{
		stringstream filename;
		filename << "results\\" << count << ".png";
		Tools::draw_stroke(filename.str().c_str(),retval[i]);
		count++;
	}

	for( int node_index = 0; node_index < (int) stroke_graph.size(); node_index++)
	{
		if (stroke_graph[node_index]->isolated)
		{
			stringstream filename;
			filename << "results\\" << count << ".png";
			vector<StrokeNode*> temp;
			temp.push_back(stroke_graph[node_index]);
			Tools::draw_stroke(filename.str().c_str(),temp);
			count++;
		}
	}

	return retval;
}

bool StrokeBuilder::decide(int x0, float x1, float x2, float x3, float x4)
{
	if (x2 >= 0.62452)
	{
		return true;
	}
	else if (x1 <= 0.61321 && x2 >= 0.26625 && x0 >= 4)
	{
		return true;
	}
	else if (x1 <= 0.61321 && x2 >= 0.08993 && x1 >= 0.55358 && x3 <= 2.107)
	{
		return true;
	}
	else if (x1 <= 0.44292 && x2 >= 0.11066 && x4 <= 0.03014)
	{
		return true;
	}
	else if (x2 >= 0.26625 && x1 <= 0.48929 && x4 >= 0.04327)
	{
		return true;
	}
	else
	{
		return false;
	}
}

StrokeBuilder::~StrokeBuilder(void)
{
	
}
